clc;clear;close all;addpath("..\..\..\Algorithm\GoldsteinUnwrap2D_r1");
I = readFigure('..\595348\Pic_');
mask_filepath = '..\mask.bmp';
Area = [1850,1850+500-1,1280,1280+500-1];
load('..\mask.mat');
for f = 1:length(I)
        I{f,1} = I{f,1}(Area(1):Area(2),Area(3):Area(4));
        I{f,1} = I{f,1}.*MASK;
end
[M,N] = size(I{1,1});
warpPhase = zeros(M,N,3);
lambda = [1/59 1/53 1/48];
lambda12 = lambda(2) * lambda(1) / (lambda(2) - lambda(1));
lambda23 = lambda(2) * lambda(3) / (lambda(3) - lambda(2));
lambda123 = lambda23 * lambda12 / (lambda23 - lambda12);
for i = 1:3
    index = (i-1)*4;
    warpPhase(:,:,i) = atan2(I{4+index,1} - I{2+index,1}, I{1+index,1} - I{3+index,1});  
end

PHI_C1 = warpPhase(:,:,1);
PHI_C2 = warpPhase(:,:,2);
% save('stdPhi.mat','PHI_C2');
PHI_C3 = warpPhase(:,:,3);
% unWarpPhase = GoldsteinUnwrap2D_r1(PHI_C1, MASK);
PHI_C12 = PHI_C1 - PHI_C2;
PHI_C12(PHI_C12<0) = PHI_C12(PHI_C12<0) + 2*pi;

PHI_C23 = PHI_C2 - PHI_C3;
PHI_C23(PHI_C23<0) = PHI_C23(PHI_C23<0) + 2*pi;

PHI_C123 = PHI_C12 - PHI_C23;
PHI_C123(PHI_C123<0) = PHI_C123(PHI_C123<0) + 2*pi;

phi_c23 = PHI_C23+2*pi*round((PHI_C123*lambda123/lambda23-PHI_C23)/(2*pi));
phi_c12 = PHI_C12+2*pi*round((PHI_C123*lambda123/lambda12-PHI_C12)/(2*pi));

phi_c1 = PHI_C1+2*pi*round((phi_c12*lambda12/lambda(1)-PHI_C1)/(2*pi));
phi_c2 = PHI_C2+2*pi*round((phi_c12*lambda12/lambda(2)-PHI_C2)/(2*pi));
phi_c3 = PHI_C3+2*pi*round((phi_c23*lambda23/lambda(3)-PHI_C3)/(2*pi));

% phi_c1 = imgaussfilt(phi_c1,[5,5]);

% save('70-unwarpphase.mat','phi_c1');
refunwarpphase = nan(3648,5472);
refunwarpphase(Area(1):Area(2),Area(3):Area(4)) = phi_c1;
save('refunwarpphase.mat',"refunwarpphase");
% unWarpPhase1(Area(1):Area(2),Area(3):Area(4)) = unWarpPhase;
ptCloud = Reconstruct(refunwarpphase,59);
pcwrite(ptCloud,'59.ply');
fprintf('点云生成！\n');



function Figure = readFigure(filePath)
    img_path_list = dir(strcat(filePath,'*.bmp'));
    %dir 列出当前文件夹中的文件信息
    img_num = length(img_path_list);%获取图像总数量
    Figure = cell(img_num,1);
    if img_num > 0 %有满足条件的图像
        for j = 1 : (img_num) %逐一读取图像
            img_name = [filePath,int2str(j),'.bmp'];
            fprintf("读取图片：%s \n",img_name);
            I = imread(img_name);
            Figure{j,1} = double(I);
        end
    end
    fprintf("------------读取图片完成-------------- \n");
end